package crtl;

import java.util.Queue;

import com.cyberbotics.webots.controller.DifferentialWheels;
import com.cyberbotics.webots.controller.DistanceSensor;
import com.cyberbotics.webots.controller.LightSensor;

import org.apache.commons.collections4.queue.CircularFifoQueue;


public abstract class EPuckCtrl extends DifferentialWheels {	
	
	protected final int _sensors = 8;
	protected final int _step = 30;
	protected final int _averageValues = 4;
	protected final int _maxSpeed = 400;
	protected final int _minSpeed = 0;
	
	private LightSensor[] _lightSensors;
	private DistanceSensor[] _distanceSensors;
	private Queue<double[]> _averageBuffer;
	
	
  	public EPuckCtrl() {
		super();
		
		 _lightSensors = new LightSensor[_sensors];
		 _distanceSensors = new DistanceSensor[_sensors];
		 _averageBuffer = new CircularFifoQueue<>(_averageValues);
		
		// Init sensors
		initLightSensors();
		initDistanceSensors();
	}
	
	/**
	 * Initialize light sensors
	 */
	private void initLightSensors() {
		for (int i = 0; i < _sensors; i++) {
			_lightSensors[i] = getLightSensor("ls" + i);
			_lightSensors[i].enable((int)getBasicTimeStep());
		}
		
	}
	
	/**
	 * Initialize distance sensors
	 */
	private void initDistanceSensors() {
		for (int i = 0; i < _sensors; i++) {
			_distanceSensors[i] = getDistanceSensor("ps" + i);
			_distanceSensors[i].enable((int)getBasicTimeStep());
		}
		
	}
	
	/**
	 * Main loop: step by step
	 * - check if finish condition is reached
	 * - navigate
	 * 
	 */
	public void run() {
		setSpeed(_minSpeed, _minSpeed);
		
		while ((step(_step) != -1) && !finished()) {
			
			Speed speed = navigate();
			if (speed != null) {
				setSpeed(speed.getLeft() * _maxSpeed, speed.getRight() * _maxSpeed);
			} else {
				System.out.println("Error: speed is null");
			}
			
		}

		System.out.println("Finished!");
		// Enter here exit cleanup code
	}
	
	
	/**
	 * Read values from light sensors
	 * 
	 * @return current values
	 */
	protected double[] readLightSensors() {
		int i = 0;
		double[] lightValues = new double[_sensors];
		for (LightSensor l : _lightSensors) {
			lightValues[i++] = l.getValue();
		}
		return lightValues;
	}
	
	/**
	 * Read values from distance sensors
	 * 
	 * @return current values
	 */
	protected double[] readDistanceSensors() {
		int i = 0;
		double[] distanceValues = new double[_sensors];
		for (DistanceSensor d : _distanceSensors) {
			distanceValues[i++] = d.getValue();
		}
		return distanceValues;
	}
	
	/**
	 * Calculate an average value of last FIFO_SIZE sensor values
	 * 
	 * Eliminates sensor hissing/noise
	 * 
	 * @param s
	 * @return 
	 */
	protected double[] calcAverage(double[] s) {
		_averageBuffer.add(s);
		double[] rv = new double[s.length];
		for (double[] buffer : _averageBuffer) {
			for (int i = 0; i < s.length; i++) {
				rv[i] += buffer[i];
			}
		}
		
		for (int i = 0; i < _sensors; i++) {
			rv[i] /= _averageValues;
		}
		return rv;
	}
	
	
	/**
	 * Normalize a vector --> transfer values to 0..1
	 * 
	 * @param s
	 * @return 
	 */
	protected double[] normalize(double[] s) {
		
		// get max and min value
		double max = s[0];
		double min = s[0];
		for (int i = 1; i < s.length; i++) {
			if (s[i] < min) {
				min = s[i];
			}
			if (s[i] > max) {
				max = s[i];
			}
		}
		
		return doNormalize(s, min, max);
	}
	
	/**
	 * Normalize a vector by given min and max values
	 * 
	 * All values over and under min/max will be set to 0 or 1
	 * 
	 * @param s
	 * @param min
	 * @param max
	 * @return 
	 */
	protected double[] normalize(double s[], double min, double max) {
		return doNormalize(s, min, max);
	}
	
	
	private double[] doNormalize(double[] s, double min, double max) {
		double[] rv = new double[s.length];
		
		// normalize
		double avg = max - min;
		for (int i = 0; i < s.length; i++) {
			rv[i] = (s[i] - min) / avg;
			
			// check if values between 0 and 1, otherwise set it to 0 or 1
			if (rv[i] < 0) {
				rv[i] = 0;
			} else if (rv[i] > 1) {
				rv[i] = 1;
			}
		}
		
		return rv;
	}
	
	/**
	 * Invert the given normalized vector:
	 *   result[i] = 1 - s[i]
	 * 
	 * @param s
	 * @return 
	 */
	protected double[] invertVector(double[] s) {
		double[] rv = new double[s.length];
		for (int i = 0; i < s.length; i++) {
			rv[i] = 1 - s[i];
		}
		return rv;
	}
	
	/**
	 * Returns the max value of given array
	 * 
	 * @param values
	 * @return 
	 */
	protected double findMax(double[] values) {
		double max = values[0];
		for (int i = 1; i < values.length; i++) {
			if (values[i] > max) {
				max = values[i];
			}
		}
		return max;
	}
	
	/**
	 * Returns true if epuck has reached his goal
	 * 
	 * @return 
	 */
	protected abstract boolean finished();
	
	/**
	 * Returns the speed for this step
	 * 
	 * @return 
	 */
	protected abstract Speed navigate();

}
